#include "buddy.h"

#define ERROR(format...)		fprintf(stderr, "[ERR] "format)
#define DEBUG(format...)		fprintf(stdout, "[DBG] "format)
#define INFO(format...)			fprintf(stdout, format)

#define TAG_USED		(0)
#define TAG_UNUSED		(1)

typedef struct _node_t {
	struct _node_t *llink;
	struct _node_t *rlink;
	uint32_t idx;
} node_t;

typedef struct {
	uint32_t size;
	node_t head;
} list_t; 

static uint8_t *g_start_address = NULL;
static list_t g_freelist[LEVEL_SIZE + 1];
static alloc_func_t g_alloc_hook = NULL;
static free_func_t g_free_hook = NULL;

void *buddy_init(alloc_func_t alloc_hook, free_func_t free_hook)
{
	uint32_t idx = 0;
	uint32_t size = 1 << LEVEL_SIZE;

    if (alloc_hook == NULL || free_hook == NULL) {
        ERROR("hook functions must be implemented\r\n");
        return NULL;
    }

    g_alloc_hook = alloc_hook;
    g_free_hook = free_hook;

	g_start_address = (uint8_t *)g_alloc_hook(size * sizeof(uint8_t));

	for (idx = 0; idx <= LEVEL_SIZE; idx++) {
		g_freelist[idx].head.llink 	= NULL;
		g_freelist[idx].head.rlink 	= NULL;
		g_freelist[idx].head.idx 	= -1;
		g_freelist[idx].size		= 1 << idx;
	}

	//init unused node
	node_t *p   = (node_t *)g_start_address;
	p->llink    = &g_freelist[LEVEL_SIZE].head;
	p->rlink    = NULL;
	p->idx      = 0;
	g_freelist[LEVEL_SIZE].head.rlink = p;

	return g_start_address;
}

uint8_t is_pow_of_2(uint32_t size)
{
	return !(size & (size - 1));
}

uint32_t next_pow_of_2(uint32_t size)
{
	if (is_pow_of_2(size)) return size;

	size |= size >> 1;
	size |= size >> 2;
	size |= size >> 4;
	size |= size >> 8;
	size |= size >> 16;

	size += 1;
	return size;
}

void remove_free_node(node_t *node)
{
    node->llink->rlink = node->rlink;
    if (node->rlink) {
        node->rlink->llink = node->llink;
    }
}

void insert_free_node(node_t *head, node_t *node)
{
    node_t *p       = head->rlink;
    head->rlink     = node;
    node->llink     = head;
    node->rlink     = p;
    if (p) {
        p->llink    = node;
    }
}

void spilt_free_node(node_t *node, uint32_t idx)
{
	if (idx == 0) {
		ERROR("size is zero, cannt spilt this node\r\n");
		return;
	}

	//remove from the prev level
	remove_free_node(node);
	
	//insert into the next level
	uint32_t parent_idx = node->idx;
	node_t *p_left = node;
	node_t *p_right = (node_t *)((uint8_t *)node + (1 << (idx-1)));

	p_left->idx     = parent_idx * 2 + 1;
	p_right->idx    = parent_idx * 2 + 2;
	p_left->llink	= NULL;
	p_left->rlink	= p_right;
	p_right->llink	= p_left;
	p_right->rlink	= NULL;

    //insert head of freelist
    node_t *p = g_freelist[idx-1].head.rlink;
    g_freelist[idx-1].head.rlink    = p_left;
    p_left->llink                   = &g_freelist[idx-1].head;
    p_right->rlink                  = p;
    if (p) {
        p->llink                    = p_right;
    }
}

void *buddy_alloc(uint32_t size, ret_t *ret)
{
	node_t *p = NULL;
	uint32_t idx = 0;
	
    if (size < sizeof(node_t)) {
        size = sizeof(node_t);
    }

	size = next_pow_of_2(size);

	for (idx = 0; idx <= LEVEL_SIZE; idx++) {
		if (g_freelist[idx].size == size) {
			//find unused node in freelist
            p = g_freelist[idx].head.rlink;
            if (p != NULL) {
				ret->offset  = (uint32_t)p - (uint32_t)g_start_address;
				ret->list_idx = idx;
                ret->node_idx = p->idx;
				remove_free_node(p);
                return p;
            }
		} else if (g_freelist[idx].size > size) {
            p = g_freelist[idx].head.rlink;
			if (p != NULL) {
				spilt_free_node(p, idx);		
				idx -= 2; //return to prev level
			}
		}
	}

	return NULL;
}

node_t *combine_free_node(node_t  *org_node, node_t  *free_node)
{
    node_t *comb_node = NULL;

    remove_free_node(org_node);

	if (org_node->idx < free_node->idx) {
		comb_node = org_node;
	} else {
		comb_node = free_node;
	}

	comb_node->idx = comb_node->idx >> 1;

	return comb_node;
}

void buddy_free(ret_t *ret)
{
	node_t *p = NULL;
	node_t *cur_p = NULL;
	uint32_t list_idx = ret->list_idx;
	uint32_t node_idx = ret->node_idx;
	uint32_t child_node_idx = node_idx - 1 + 2 * (node_idx & 1);

	cur_p = (node_t *)(g_start_address+ ret->offset);
	cur_p->idx = node_idx;

	for (; list_idx <= LEVEL_SIZE; list_idx++) {
		p = &g_freelist[list_idx].head;
		p = p->rlink;

		while (p) {
			if (p->idx == child_node_idx) {
				//unused node
				cur_p = combine_free_node(p, cur_p);
				child_node_idx = cur_p->idx - 1 + 2 * (cur_p->idx & 1);
				goto NEXT;
			}
			p = p->rlink;
		}

		//child node is used, just insert free node
		insert_free_node(&g_freelist[list_idx].head, cur_p);
		return;
	NEXT:
		p = NULL;
	}
}

void buddy_destroy()
{
    g_free_hook(g_start_address);
}

void buddy_dump()
{
	uint32_t idx = 0;
	node_t *p = NULL;

	for (idx = 0; idx <= LEVEL_SIZE; idx++) {
		INFO("FreeList[%02d] size: %d\r\n", idx, g_freelist[idx].size);
		p = &g_freelist[idx].head;
		while (p->rlink) {
			INFO("\t [%02d]: %6s offset: 0x%x\r\n", p->rlink->idx, "UNUSED", (uint32_t)p->rlink - (uint32_t)g_start_address);
			p = p->rlink;
		}
		INFO("-----------------------\r\n");
	}
}


